
#include "lib_qbar.h"
#include <Wire.h>
#include "definitions.h"
#include <avr/pgmspace.h>

long lastReset;
long lastNetCheck;

QB qb;
Xbee xbee;

State*            activeState;
SleepState        sleepState(&qb);
SoundState        soundState(&qb);
PlayState         playState(&qb);
ColorSmackState   colorSmackState(&qb);
PuzzleState       puzzleState(&qb);
ScriptSelectState scriptSelectState(&qb, &soundState, &colorSmackState, &puzzleState);
IdentifyState     identifyState(&qb);

int twoThreePattern[] = {100, 50, 50, 50, 0, 0, 0, 0, 0, 0};

ClapTransition        doubleClap(2, &qb);
TimeTransition        time1min(1000L*60L*1L); //1 min
TimeTransition        time2min(1000L*60L*2L); //2 mins
TimeTransition        time3min(1000L*60L*3L); //3 mins
TimeTransition        time4min(1000L*60L*4L); //4 mins
TimeTransition        time5min(1000L*60L*5L); //5 mins
MoveTransition        move(40, &qb);
CommandTransition     goToSense(&qb,  COMMAND_SCRIPT_SENSE);
CommandTransition     goToPlay(&qb,   COMMAND_STATE_PLAY);
CommandTransition     goToCreate(&qb, COMMAND_SCRIPT_CREATE);
CommandTransition     goToPuzzle(&qb, COMMAND_SCRIPT_PUZZLE);
CommandTransition     goToSleep(&qb, COMMAND_SLEEP);
CommandTransition     goToIdentify(&qb, COMMAND_IDENTIFY);

//----Stuff for xbee adress stuff
prog_char string_0[] PROGMEM = "00000000";  //XX0
prog_char string_1[] PROGMEM = "4076970E"; 	//XX1  -> A
prog_char string_2[] PROGMEM = "406FC95D";	//XX2  -> B 
prog_char string_3[] PROGMEM = "407A3938";  //XX3  -> C
prog_char string_4[] PROGMEM = "407A7DFF";  //XX4  -> D
prog_char string_5[] PROGMEM = "40764776";  //XX5  -> E
prog_char string_6[] PROGMEM = "407A3921";  //XX6  -> F
prog_char string_7[] PROGMEM = "407A38F8";  //XX7  -> G 
prog_char string_8[] PROGMEM = "4076475D";  //XX8  -> H
prog_char string_9[] PROGMEM = "407695AA";  //XX9  -> I 
prog_char string_10[] PROGMEM = "407696E1"; //XX10 -> J
prog_char string_11[] PROGMEM = "4076949C"; //XX11 -> K
prog_char string_12[] PROGMEM = "407A7DFB"; //XX12 -> L 
prog_char string_13[] PROGMEM = "406FCAAB"; //XX13 -> M
prog_char string_14[] PROGMEM = "407A7E35"; //XX14 -> N
prog_char string_15[] PROGMEM = "40764765"; //MUSEUMCUBE X1
prog_char string_16[] PROGMEM = "407692E7"; //MUSEUMCUBE X2
prog_char string_17[] PROGMEM = "40769246"; //MUSEUMCUBE XXX4  //byttet for å få den inn på nettet X4(407691FE)
prog_char string_18[] PROGMEM = "407A7E05"; //MUSEUMCUBE X5
prog_char string_19[] PROGMEM = "407692E5"; //MUSEUMCUBE X6
prog_char string_20[] PROGMEM = "40769285"; //USED AS RESET , WILL NEVER SHOW UP

PROGMEM const char *xbeeadress_table[] = 	   // change "string_table" name to suit
{   
	string_0, string_1, string_2, string_3, string_4, string_5, string_6, string_7, string_8, string_9,
		string_10,string_11,string_12,string_13,string_14,string_15,string_16,string_17,string_18,string_19,
		string_20
	};

	char xbeeAdressbuffer[9]; 
//----

	void setup()  {
		randomSeed(analogRead(4));
		Serial.begin(115200); //Xbee må ha denne på serialzen frå no avs

		delay(1000); // Delay for å sikre at blinkm-ene starter ordentlig opp før de blir resatt
		lastReset = millis();

		qb.prepare();
		#ifdef USEXBEE
		qb.prepareAndSetupXbee();
		#endif
		assignQBidFromXbeeAdress();
		lastNetCheck = millis();

// *** Oppsett av regier ***
		sleepState.addTransition(     &doubleClap, &scriptSelectState);
		sleepState.addTransition(     &move,       &scriptSelectState);
		sleepState.addTransition(     &goToSense,  &soundState);
		sleepState.addTransition(     &goToCreate, &colorSmackState);
		sleepState.addTransition(     &goToPuzzle, &puzzleState);
		sleepState.addTransition(     &goToPlay,   &playState);

		soundState.addTransition(     &time2min,  &playState);
		playState.addTransition(      &time3min,  &sleepState);
		colorSmackState.addTransition(&time5min,  &sleepState);
		puzzleState.addTransition(    &time5min,  &sleepState);

// Tvungen identifisering
		soundState.addTransition(     &goToIdentify, &identifyState);
		playState.addTransition(      &goToIdentify, &identifyState);
		colorSmackState.addTransition(&goToIdentify, &identifyState);
		puzzleState.addTransition(    &goToIdentify, &identifyState);
		sleepState.addTransition(     &goToIdentify, &identifyState);

// Tvungen innsovning
		soundState.addTransition(     &goToSleep, &sleepState);
		playState.addTransition(      &goToSleep, &sleepState);
		colorSmackState.addTransition(&goToSleep, &sleepState);
		puzzleState.addTransition(    &goToSleep, &sleepState);
		identifyState.addTransition(  &goToSleep, &sleepState);

// *** Oppstartsstate ***
		activeState = &sleepState;
		activeState->enter();

		Serial.print("\nQB");
		Serial.println(qb.getQbId());
	} 

//Setup end

	void loop()  {
		qb.updateSensors(); 
		activeState = activeState->execute();
		// Oppsett av Xbee om vi ikke har ID
		if (millis() - lastNetCheck > 1000L*60 && !isProperQbId(qb.getQbId())) {
			#ifdef USEXBEE
			qb.prepareAndSetupXbee();
			assignQBidFromXbeeAdress();
			#endif
			lastNetCheck = millis();
		}
	}

	boolean isProperQbId(char id) {
		return id >= 65 && id <= 70; // Ascii A-Z
	}


	void assignQBidFromXbeeAdress() {
		for(int i = 0; i < 21; i++){ //Lenght of our progmem array
			strcpy_P(xbeeAdressbuffer, (char*)pgm_read_word(&(xbeeadress_table[i]))); 
			delay(35);
			if(qb.getXbeeAdress().equals(xbeeAdressbuffer)){
				qb.setQbId(i+64);
				break;
			}
		}
	}
